#include "precompiled.h"
#include "PhysicsManager.h"
#include "World_Manager.h"
#include "Scenery.h"
#include "Player.h"
#include "Camera_Alt.h"
#include "Sound_Manager.h"
#include "GameCommon.h"

//.....................................................................
// [id=keycode]

#include <Common/Base/keycode.cxx>

// [id=productfeatures]
// We're using only physics - we undef products even if the keycode is present so
// that we don't get the usual initialization for these products.

#undef HK_FEATURE_PRODUCT_AI
#undef HK_FEATURE_PRODUCT_ANIMATION
#undef HK_FEATURE_PRODUCT_CLOTH
#undef HK_FEATURE_PRODUCT_DESTRUCTION
#undef HK_FEATURE_PRODUCT_BEHAVIOR
//#define HK_EXCLUDE_FEATURE_SerializeDeprecatedPre700
//#define HK_EXCLUDE_FEATURE_RegisterVersionPatches

// Vdb needs the reflected classes
#define HK_EXCLUDE_FEATURE_MemoryTracker
#define HK_EXCLUDE_FEATURE_hkpAccurateInertiaTensorComputer
#define HK_EXCLUDE_FEATURE_CompoundShape

#define HK_EXCLUDE_FEATURE_hkpAabbTreeWorldManager
#define HK_EXCLUDE_FEATURE_hkpContinuousSimulation
#define HK_EXCLUDE_FEATURE_hkpKdTreeWorldManager

#include <Common/Base/Config/hkProductFeatures.cxx>
//.....................................................................

//.....................................................................
#include "tbb/concurrent_vector.h"
using namespace tbb;
//.....................................................................


//.....................................................................
#include "PhysicsCommon.h"
#include "Physics_Character.h"
#include "Physics_Scenery.h"

const int NUM_THRAEDS_PHYSICS = 1;

const std::string MAP_WTEST_1_HAVOK("../models/HAVOK/room_0.hkt");
//.....................................................................




namespace PhysicsManager{
	
	bool running;
	std::vector<Scenery> scenery;

	// The havok world.
	hkpWorld *world;

	// A collection of havok object id's for setting colors in the vdb.
	hkArray<hkUlong> m_objectIds;

	// The player character.
	Physics_Character *ptr_player_character;

	// The scenery.
	//tbb::concurrent_vector<Physics_Scenery*> vec_scenery;

	std::vector<Physics_Scenery*> unavailable_scenery;
	std::deque<Physics_Scenery*> available_scenery;

	// The static environment.
	Physics_Scenery *ptr_environment;

	typedef struct{
		Particle *p;
		hkMovingRBCollectorPhantom *phantom;
	}paticle_phantom_request;
	std::stack<paticle_phantom_request> bullet_phantom_requests;
	std::vector<hkMovingRBCollectorPhantom*> bullet_phantoms;

	 void HK_CALL errorReport(const char* msg, void* userContext)
	{
		printf("%s\n", msg);
	}
	 

	 void addFixedSurface(hkpWorld* world, const hkVector4& position, const hkVector4& dimensions)
	 {
		 hkReal m_fhkConvexShapeRadius = (hkReal) 0.05;
		 hkpShape* fixedSurfaceShape = new hkpBoxShape(dimensions,m_fhkConvexShapeRadius);

		 //create rigid body information structure
		 hkpRigidBodyCinfo m_rigidBodyInfo;

		 //MOTION_FIXED means static element in game scene
		 m_rigidBodyInfo.m_mass = 10.0;
		 m_rigidBodyInfo.m_shape = fixedSurfaceShape;
		 m_rigidBodyInfo.m_motionType = hkpMotion::MOTION_FIXED;
		 m_rigidBodyInfo.m_position = position;

		 //create a new rigid body with supplied info
		 hkpRigidBody* m_pRigidBody = new hkpRigidBody(m_rigidBodyInfo);

		 //add rigid body to the world
		 world->addEntity (m_pRigidBody);

		 //decrease reference counter for rigid body and shape
		 m_pRigidBody->removeReference();
		 fixedSurfaceShape->removeReference();
	 }

	 // Give us a static plane and a bouncing ball.
	 void HK_CALL dbg_addBodies(){

		 // Bouncing sphere
		 /*hkpRigidBody* rigidBody;
		 {
			 // Create a sphere with radius 2.0 m
			 hkpSphereShape* sphereShape = new hkpSphereShape(2.0f);

			 // Sphere information
			 hkpRigidBodyCinfo bodyCinfo;

			 // spherical shape
			 bodyCinfo.m_shape = sphereShape;

			 // start position
			 bodyCinfo.m_position = hkVector4(0.0,15.0,0.0,0.0);

			 //  Calculate the mass properties for the shape
			 const hkReal sphereMass = 10.0f; // mass of the sphere
			 hkpMassProperties massProperties; // output variable

			 // Compute the inertia tensor (e.g. dynamic information)
			 hkpInertiaTensorComputer::computeShapeVolumeMassProperties(
				 sphereShape, sphereMass, massProperties);

			 // Update mass properties
			 bodyCinfo.setMassProperties(massProperties);

			 // Elasticity (value between 0 and 1.99, default = 0.4)
			 bodyCinfo.m_restitution = (hkReal) 1.9;

			 // Create the rigid body
			 rigidBody = new hkpRigidBody(bodyCinfo);

			 // No longer need the reference on the sphereShape, as the rigidBody now owns it
			 sphereShape->removeReference();
		 }

		 // Add the rigidBody to the world
		 world->addEntity(rigidBody);
		 */

		 // Add a static floor
		 addFixedSurface(world, hkVector4(0.0,-10.0,0.0,0.0), hkVector4(90.0,1.0,90.0,1.0));

	 }

	 void HK_CALL PhysicsManager::Init_Havok(){

		 hkMallocAllocator baseMalloc;

		 // Need to have memory allocated for the solver. Allocate 1mb for it.
		 hkMemoryRouter* memoryRouter = hkMemoryInitUtil::initDefault(
			 &baseMalloc, hkMemorySystem::FrameInfo(1024 * 1024) );

		 hkBaseSystem::init( memoryRouter, errorReport );
	}
	 void HK_CALL PhysicsManager::Cleanup_Havok(){

		hkBaseSystem::quit();
		hkMemoryInitUtil::quit();
	}

	
	 void HK_CALL PhysicsManager::createWorld(){}

	 void HK_CALL PhysicsManager::cleanupWorld(){}

	 void HK_CALL PhysicsManager::add_RigidBody_Player(){
		 ptr_player_character = new Physics_Character();

		 ptr_player_character->Create( world );
	 }

	 void HK_CALL PhysicsManager::add_RigidBody_Scenery(  ){
		 while( !World_Manager::async_requestRigidbodies_Scenery.empty() ){
			 Physics_Scenery *ps = new Physics_Scenery();
			 ps->Create( world, World_Manager::async_requestRigidbodies_Scenery.top() );
			 //vec_scenery.push_back( ps );
			 unavailable_scenery.push_back( ps );
			 World_Manager::async_requestRigidbodies_Scenery.pop();
		 }
	}

	 void HK_CALL Manage_BulletHit(hkpAabbPhantom* ref_phantom, Physics_Scenery* ref_scenery){
		 //printf("Bullet hit a game object!\n");
		 ref_scenery->setDoFlash();
		 ref_scenery->getScenery()->lowerHealth( DAMAGE_PULSERIFLE );

		 SoundManager::OrderASound( "shot_landed", false );
	 }

	 // Create and add a phantom to the world.
	 void HK_CALL add_Phantom_Particle(hkMovingRBCollectorPhantom* phantom, Particle *p){

		 world->lock();

		 hkAabb infoAABB;
		 hkReal halfWidth = 0.1f;
		 infoAABB.m_min.set( -halfWidth + p->pos.x, 0.0f + p->pos.y, -halfWidth + p->pos.z );
		 infoAABB.m_max.set( halfWidth + p->pos.x, 20.0f + p->pos.y, halfWidth + p->pos.z ) ;


		 if(phantom == 0){
			 phantom = new hkMovingRBCollectorPhantom( infoAABB );
			 phantom->set_particle( p );
		 }

		 // To "move" a Phantom since it is (in the current API) just an AABB, we use the
		 // setAabb() method to explicitly set its new location in the World. It
		 // automtically updates its own list of entities which it currently overlaps.
		 phantom->setAabb( infoAABB );

		 world->addPhantom( phantom );
		 bullet_phantoms.push_back( phantom );

		 world->unlock();
	 }
	 void HK_CALL answer_bulletPhantom_requests(){
		 while( !bullet_phantom_requests.empty() ){
			 add_Phantom_Particle( bullet_phantom_requests.top().phantom, bullet_phantom_requests.top().p );
			 bullet_phantom_requests.pop();
		 }
	 }
	 void HK_CALL add_bulletPhantom_requests(hkMovingRBCollectorPhantom* phantom, Particle *p){
		 paticle_phantom_request r = {p,phantom};
		 bullet_phantom_requests.push(r);
	 }
	 
	 void HK_CALL update_bulletPhantoms( hkReal timestep ){

		 world->lock();

		 bool hit = false;
		 hkAabb infoAABB;
		 hkReal halfWidth = 0.1f;
		 for( int c = 0; c < bullet_phantoms.size(); c++ ){
			 
			 // Update the phantom's position.
			 glm::vec3 position = bullet_phantoms.at(c)->get_particle()->pos;
			 infoAABB.m_min.set( -halfWidth + position.x, 0.0f + position.y, -halfWidth + position.z );
			 infoAABB.m_max.set( halfWidth + position.x, 20.0f + position.y, halfWidth + position.z ) ;
			 bullet_phantoms.at(c)->setAabb( infoAABB );

			 // Mark each body this phantom has as hit by this bullet.
			 bullet_phantoms.at(c)->ensureDeterministicOrder();
			 for (int i = 0; i < bullet_phantoms.at(c)->getOverlappingCollidables().getSize(); i++ )
			 {
				 // Note: it is OK to directly cast here, because we filter out all but moving rigid bodies in the add and remove collidable calls (above)
				 hkpRigidBody* rb = hkpGetRigidBody(bullet_phantoms.at(c)->getOverlappingCollidables()[i]);

				 // Get the game object for this body.
				 Physics_Scenery* ptr_scenery=0;
				 ptr_scenery = RigidBodyGameObjectMap::get_scenery(rb->m_uid);

				 if( ptr_scenery ){
					 hit=true;
					 Manage_BulletHit(bullet_phantoms.at(c),ptr_scenery);
				 }
			 }

			 // Remove this phantom as it's bullet is "used up".
			 if( hit ){
				 bullet_phantoms.at(c)->get_particle()->alive=false;
				 bullet_phantoms.erase( bullet_phantoms.begin() + c );
				 c--;
			 }
			 hit=false;
		 }

		 world->unlock();
	 }

	 // Compute a plane equation from triangle vertices, store offset to the origin in W.
	 static HK_FORCE_INLINE hkVector4	computePlaneFromTriangle(const hkVector4& a, const hkVector4& b, const hkVector4& c)
	 {
		 hkVector4	ab; ab.setSub4(b,a);
		 hkVector4	ac; ac.setSub4(c,a);
		 hkVector4	plane;
		 plane.setCross(ab, ac);
		 plane.normalize3();
		 plane(3) = -plane.dot3(a);
		 return plane;
	 }
	 
	 // Old.
	 /*
	 // Make note of what scenery falls outside of our view frustum.
	 void HK_CALL cull_scenery( hkpWorld* world,
		 tbb::concurrent_vector<Physics_Scenery*> vec_scenery,
		 std::vector<hkUint32>* ids_in_frustum ){

			 Camera_Alt const* pcam = World_Manager::GetPlayereCamera();
			 glm::vec3 cpos = pcam->getPosition();
			 hkVector4 trans(cpos.x,cpos.y,cpos.z,0);

			 // Lock the world and get the tree broad-phase.
			 world->lock();
			 hkpTreeBroadPhase*	broadphase = static_cast<hkpTreeBroadPhase*>(world->getBroadPhase());
			 //broadphase->fullOptimize();


			 glm::vec3 f_r = -pcam->getXAxis();
			 glm::vec3 f_u = -pcam->getYAxis();
			 glm::vec3 f_f = -pcam->getZAxis();

			 // Rotate our forward a bit to the right?
			 //f_f = glm::rotateY(f_f, -10.0f);

			 // organize our axes...................................
			 float sc = 50.0f;
			 hkVector4 start_fu( f_u.x, f_u.y, f_u.z, 1 );
			 hkVector4 end_fu( f_u.x*sc, f_u.y*sc, f_u.z*sc, 1 );
			 hkVector4 start_fr( f_r.x, f_r.y, f_r.z, 1 );
			 hkVector4 end_fr( f_r.x*sc, f_r.y*sc, f_r.z*sc, 1 );
			 hkVector4 start_ff( f_f.x, f_f.y, f_f.z, 1 );
			 hkVector4 end_ff( f_f.x*sc, f_f.y*sc, f_f.z*sc, 1 );
			 start_fu.add(trans);
			 end_fu.add(trans);
			 start_fr.add(trans);
			 end_fr.add(trans);
			 start_ff.add(trans);
			 end_ff.add(trans);
			 //.....................................................
			 if( VISUALIZE_AXES ){
				 // Visualize the axes in the vdb........................
				 HK_DISPLAY_LINE( start_fu, end_fu ,hkColor::RED);
				 HK_DISPLAY_LINE( start_fr, end_fr ,hkColor::AZURE);
				 HK_DISPLAY_LINE( start_ff, end_ff ,hkColor::GREEN);
			 }
			 //........................................................

			 // Create frustum vertices.
			 hkVector4	nearPlaneQuad[4];

			 // Find corners according to our direction.
			 float s1=10.0f;
			 glm::vec3 bl( f_r*-s1 + f_u*-s1 );
			 glm::vec3 br( f_r*s1 + f_u*-s1 );
			 glm::vec3 tr( f_r*s1 + f_u*s1 );
			 glm::vec3 tl( f_r*-s1 + f_u*s1 );

			 nearPlaneQuad[0].set( bl.x, bl.y, bl.z );
			 nearPlaneQuad[1].set( br.x, br.y, br.z );
			 nearPlaneQuad[2].set( tr.x, tr.y, tr.z );
			 nearPlaneQuad[3].set( tl.x, tl.y, tl.z );

			 hkVector4	farPlaneQuad[4];
			 hkSimdReal	fovFactor = pcam->getFov() * 6.5f;
			 hkReal		farOffset = pcam->getZFar();
			 for(int i=0; i<4; ++i)
			 {
				 farPlaneQuad[i].setMul4(fovFactor, nearPlaneQuad[i]);

				 hkVector4 dist;
				 dist.setMul4( farOffset, hkVector4(f_f.x,f_f.y,f_f.z,0) );
				 farPlaneQuad[i].add( dist );


				 // Translate frustum.
				 nearPlaneQuad[i].add( trans );
				 farPlaneQuad[i].add( trans );
			 }



			 // Compute frustum planes.
			 hkVector4	planes[6];		

			 // Side planes
			 planes[0] = computePlaneFromTriangle(nearPlaneQuad[0], farPlaneQuad[0], nearPlaneQuad[1]);  //b
			 planes[1] = computePlaneFromTriangle(nearPlaneQuad[1], farPlaneQuad[1], nearPlaneQuad[2]);  //r
			 planes[2] = computePlaneFromTriangle(nearPlaneQuad[2], farPlaneQuad[2], nearPlaneQuad[3]);  //t
			 planes[3] = computePlaneFromTriangle(nearPlaneQuad[3], farPlaneQuad[3], nearPlaneQuad[0]);  //l

			 // Visualize the frustum.
			 if( VISUALIZE_FRUSTUM ){
				 //
				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[0],farPlaneQuad[0],nearPlaneQuad[1], hkColor::HOTPINK);
				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[1],farPlaneQuad[1],farPlaneQuad[0], hkColor::HOTPINK);

				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[1],nearPlaneQuad[2],farPlaneQuad[1], hkColor::HOTPINK);
				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[2],farPlaneQuad[2],farPlaneQuad[1], hkColor::HOTPINK);

				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[2],nearPlaneQuad[3],farPlaneQuad[2], hkColor::HOTPINK);
				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[3],farPlaneQuad[3],farPlaneQuad[2], hkColor::HOTPINK);

				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[3],nearPlaneQuad[0],farPlaneQuad[3], hkColor::HOTPINK);
				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[0],farPlaneQuad[0],farPlaneQuad[3], hkColor::HOTPINK);

				 //
				 HK_DISPLAY_TRIANGLE(farPlaneQuad[0],farPlaneQuad[1],farPlaneQuad[2], hkColor::HOTPINK);
				 HK_DISPLAY_TRIANGLE(farPlaneQuad[2],farPlaneQuad[3],farPlaneQuad[0], hkColor::HOTPINK);

				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[0],nearPlaneQuad[1],nearPlaneQuad[2], hkColor::HOTPINK);
				 HK_DISPLAY_TRIANGLE(nearPlaneQuad[2],nearPlaneQuad[3],nearPlaneQuad[0], hkColor::HOTPINK);
			 }

			 // Near plane
			 planes[4] = computePlaneFromTriangle(nearPlaneQuad[0], nearPlaneQuad[1], nearPlaneQuad[2]);		

			 // Far plane
			 planes[5] = computePlaneFromTriangle(farPlaneQuad[0], farPlaneQuad[2], farPlaneQuad[1]);


			 if( VISUALIZE_FRUSTUM ){
				 // Display plane normals.
				 float s2 = 3.0f;
				 hkVector4 tmp1 = planes[0];
				 hkVector4 tmp2;

				 // b
				 tmp1.setMul4(s2,tmp1);
				 tmp2.setMul4(s2,tmp1);
				 HK_DISPLAY_ARROW( tmp1, tmp2, hkColor::RED );

				 // r
				 tmp1 = planes[1];
				 tmp1.setMul4(s2,tmp1);
				 tmp2.setMul4(s2,tmp1);
				 HK_DISPLAY_ARROW( tmp1, tmp2, hkColor::GREEN );

				 // t
				 tmp1 = planes[2];
				 tmp1.setMul4(s2,tmp1);
				 tmp2.setMul4(s2,tmp1);
				 HK_DISPLAY_ARROW( tmp1, tmp2, hkColor::BLUE );

				 // l
				 tmp1 = planes[3];
				 tmp1.setMul4(s2,tmp1);
				 tmp2.setMul4(s2,tmp1);
				 HK_DISPLAY_ARROW( tmp1, tmp2, hkColor::WHITE );

				 // n
				 tmp1 = planes[4];
				 tmp1.setMul4(s2,tmp1);
				 tmp2.setMul4(s2,tmp1);
				 HK_DISPLAY_ARROW( tmp1, tmp2, hkColor::YELLOW );

				 // f
				 tmp1 = planes[5];
				 tmp1.setMul4(s2,tmp1);
				 tmp2.setMul4(s2,tmp1);
				 HK_DISPLAY_ARROW( tmp1, tmp2, hkColor::CYAN );
			 }

			 broadphase->lock();
			 // Cull using frustum.
			 hkArray<const hkpBroadPhaseHandle*>	handles;
			 {
				 HK_TIME_CODE_BLOCK("Culling",this);
				 handles.reserve(4096 / sizeof(const hkpBroadPhaseHandle*));
				 broadphase->queryConvex(planes, 6, handles);
			 }

			 // Get out objects visible from the frustum.
			 for(int i=0; i<handles.getSize(); ++i)
			 {
				 ids_in_frustum->push_back( handles[i]->m_id );
			 }

			 broadphase->unlock();
			 world->unlock();
	 }*/

// New.
// Make note of what scenery falls outside of our view frustum.
void HK_CALL cull_scenery( hkpWorld* world,
	//tbb::concurrent_vector<Physics_Scenery*> vec_scenery,
	std::vector<Physics_Scenery*> vec_scenery,
	std::vector<hkUint32>* ids_in_frustum ){

		Camera_Alt const* pcam = World_Manager::GetPlayereCamera();
		glm::vec3 look_from = World_Manager::player->get_lookFrom();
		glm::vec3 look_to = World_Manager::player->get_lookTo();
		hkVector4 transFrom(look_from.x,look_from.y,look_from.z,0);
		hkVector4 transTo(look_to.x,look_to.y,look_to.z,0);

		// Lock the world and get the tree broad-phase.
		world->lock();
		hkpTreeBroadPhase*	broadphase = static_cast<hkpTreeBroadPhase*>(world->getBroadPhase());
		//broadphase->fullOptimize();


		glm::vec3 f_r = -pcam->getXAxis();
		glm::vec3 f_u = -pcam->getYAxis();
		glm::vec3 f_f = -pcam->getZAxis();

		// organize our axes...................................
		float sc = 50.0f;
		hkVector4 start_fu( f_u.x, f_u.y, f_u.z, 1 );
		hkVector4 end_fu( f_u.x*sc, f_u.y*sc, f_u.z*sc, 1 );
		hkVector4 start_fr( f_r.x, f_r.y, f_r.z, 1 );
		hkVector4 end_fr( f_r.x*sc, f_r.y*sc, f_r.z*sc, 1 );
		hkVector4 start_ff( f_f.x, f_f.y, f_f.z, 1 );
		hkVector4 end_ff( f_f.x*sc, f_f.y*sc, f_f.z*sc, 1 );
		start_fu.add(transFrom);
		end_fu.add(transFrom);
		start_fr.add(transFrom);
		end_fr.add(transFrom);
		start_ff.add(transFrom);
		end_ff.add(transFrom);
		//.....................................................
		if( VISUALIZE_AXES ){
			// Visualize the axes in the vdb........................
			HK_DISPLAY_LINE( start_fu, end_fu ,hkColor::RED);
			HK_DISPLAY_LINE( start_fr, end_fr ,hkColor::AZURE);
			HK_DISPLAY_LINE( start_ff, end_ff ,hkColor::GREEN);
		}
		//........................................................

		// Create frustum vertices.
		hkVector4	nearPlaneQuad[4];

		// Find corners according to our direction.
		float s1=10.0f;
		glm::vec3 bl( f_r*-s1 + f_u*-s1 );
		glm::vec3 br( f_r*s1 + f_u*-s1 );
		glm::vec3 tr( f_r*s1 + f_u*s1 );
		glm::vec3 tl( f_r*-s1 + f_u*s1 );

		nearPlaneQuad[0].set( bl.x, bl.y, bl.z );
		nearPlaneQuad[1].set( br.x, br.y, br.z );
		nearPlaneQuad[2].set( tr.x, tr.y, tr.z );
		nearPlaneQuad[3].set( tl.x, tl.y, tl.z );

		hkVector4	farPlaneQuad[4];
		hkSimdReal	fovFactor = pcam->getFov() * 6.5f;
		hkReal		farOffset = pcam->getZFar();
		for(int i=0; i<4; ++i)
		{
			farPlaneQuad[i].setMul4(fovFactor, nearPlaneQuad[i]);

			/*hkVector4 dist;
			dist.setMul4( farOffset, hkVector4(f_f.x,f_f.y,f_f.z,0) );
			farPlaneQuad[i].add( dist );*/


			// Translate frustum.
			nearPlaneQuad[i].add( transFrom );
			farPlaneQuad[i].add( transTo );
		}



		// Draw frustum.
		if( VISUALIZE_FRUSTUM ){
			HK_DISPLAY_LINE(nearPlaneQuad[0], nearPlaneQuad[1], hkColor::YELLOW);
			HK_DISPLAY_LINE(nearPlaneQuad[1], nearPlaneQuad[2], hkColor::YELLOW);
			HK_DISPLAY_LINE(nearPlaneQuad[2], nearPlaneQuad[3], hkColor::YELLOW);
			HK_DISPLAY_LINE(nearPlaneQuad[3], nearPlaneQuad[0], hkColor::YELLOW);

			HK_DISPLAY_LINE(farPlaneQuad[0], farPlaneQuad[1], hkColor::YELLOW);
			HK_DISPLAY_LINE(farPlaneQuad[1], farPlaneQuad[2], hkColor::YELLOW);
			HK_DISPLAY_LINE(farPlaneQuad[2], farPlaneQuad[3], hkColor::YELLOW);
			HK_DISPLAY_LINE(farPlaneQuad[3], farPlaneQuad[0], hkColor::YELLOW);

			HK_DISPLAY_LINE(farPlaneQuad[0], nearPlaneQuad[0], hkColor::YELLOW);
			HK_DISPLAY_LINE(farPlaneQuad[1], nearPlaneQuad[1], hkColor::YELLOW);
			HK_DISPLAY_LINE(farPlaneQuad[2], nearPlaneQuad[2], hkColor::YELLOW);
			HK_DISPLAY_LINE(farPlaneQuad[3], nearPlaneQuad[3], hkColor::YELLOW);
		}

		// Compute frustum planes.
		hkVector4	planes[6];		

		// Side planes
		planes[0] = computePlaneFromTriangle(nearPlaneQuad[0], farPlaneQuad[0], nearPlaneQuad[1]);
		planes[1] = computePlaneFromTriangle(nearPlaneQuad[1], farPlaneQuad[1], nearPlaneQuad[2]);
		planes[2] = computePlaneFromTriangle(nearPlaneQuad[2], farPlaneQuad[2], nearPlaneQuad[3]);
		planes[3] = computePlaneFromTriangle(nearPlaneQuad[3], farPlaneQuad[3], nearPlaneQuad[0]);

		// Near plane
		planes[4] = computePlaneFromTriangle(nearPlaneQuad[0], nearPlaneQuad[1], nearPlaneQuad[2]);		

		// Far plane
		planes[5] = computePlaneFromTriangle(farPlaneQuad[0], farPlaneQuad[2], farPlaneQuad[1]);


		broadphase->lock();
		// Cull using frustum.
		hkArray<const hkpBroadPhaseHandle*>	handles;
		{
			HK_TIME_CODE_BLOCK("Culling",this);
			handles.reserve(4096 / sizeof(const hkpBroadPhaseHandle*));
			broadphase->queryConvex(planes, 6, handles);
		}

		// Get out objects visible from the frustum.
		for(int i=0; i<handles.getSize(); ++i)
		{
			ids_in_frustum->push_back( handles[i]->m_id );
		}

		broadphase->unlock();
		world->unlock();
}

	void HK_CALL PhysicsManager::Update_UnavailableScenery( const hkReal& timeStep ){
		world->lock();

		std::vector<hkUint32> ids_in_frustum;

		if( FRUSTUM_CULLING_ON ){
			//cull_scenery( world, vec_scenery, &ids_in_frustum );
			cull_scenery( world, unavailable_scenery, &ids_in_frustum );
		}

		// Update our bullets.
		answer_bulletPhantom_requests();
		update_bulletPhantoms( timeStep );

		// Update our scenery objects.
		for( int c = 0; c < unavailable_scenery.size(); c++ ){
			unavailable_scenery.at( c )->Update( timeStep, &ids_in_frustum );

			// Remove dead scenery and put it into the available stack.
			if( unavailable_scenery.at( c )->getScenery()->isDead() ){

				unavailable_scenery.at( c )->getScenery()->doDeath();
				world->removeEntity( unavailable_scenery.at( c )->getRigidBody() );
				available_scenery.push_back( unavailable_scenery.at( c ) );
				unavailable_scenery.erase( unavailable_scenery.begin() + c );
				c--;

				printf("unavailable scenery size: %d\n", unavailable_scenery.size());
				printf("available scenery size: %d\n", available_scenery.size());
			}
		}

		world->unlock();
	}

	 void HK_CALL PhysicsManager::Update( const hkReal& timeStep ){

		 /*std::vector<hkUint32> ids_in_frustum;

		 if( FRUSTUM_CULLING_ON ){
			 //cull_scenery( world, vec_scenery, &ids_in_frustum );
			 cull_scenery( world, unavailable_scenery, &ids_in_frustum );
		 }*/

		 // Update our player.
		 ptr_player_character->Update( world, timeStep ,World_Manager::player );
		 
		 Update_UnavailableScenery( timeStep );

		 // Update our bullets.
		/* answer_bulletPhantom_requests();
		 update_bulletPhantoms( timeStep );

		 // Update our scenery objects.
		 for( int c = 0; c < vec_scenery.size(); c++ )
			 vec_scenery.at( c )->Update( timeStep, &ids_in_frustum );*/

	 }

	 void HK_CALL PhysicsManager::Update_Transforms( vector<Scenery*> &sv ){}

	 // Start up the physics loop.
	 void HK_CALL run(){
		 // Initialize.
		 running=true;

		 hkMallocAllocator baseMalloc;
		 // Need to have memory allocated for the solver. Allocate 1mb for it.
		 /*hkMemoryRouter* memoryRouter = hkMemoryInitUtil::initDefault(
			 &baseMalloc, hkMemorySystem::FrameInfo(1024 * 1024) );
		 hkBaseSystem::init( memoryRouter, errorReport );*/
		 hkMemoryRouter* memoryRouter = hkMemoryInitUtil::initDefault( 
			 hkMallocAllocator::m_defaultMallocAllocator, hkMemorySystem::FrameInfo( 1024* 1024 ) );  
		 hkBaseSystem::init( memoryRouter, errorReport );  

		 // Threading..............................................
		 hkJobThreadPool* threadPool;
		 int totalNumThreadsUsed = NUM_THRAEDS_PHYSICS;

		 // We also need to create a Job queue. This job queue will be used by all Havok modules to run multithreaded work.
		 // Here we only use it for physics.
		 hkJobQueueCinfo jqinfo;
		 jqinfo.m_jobQueueHwSetup.m_numCpuThreads = totalNumThreadsUsed;
		 hkJobQueue* jobQueue = new hkJobQueue(jqinfo);

		 //
		 // Enable monitors for this thread.
		 //

		 // Monitors have been enabled for thread pool threads already (see above comment).
		 hkMonitorStream::getInstance().resize(200000);
		 //............................................................

		 // Create World...............................................

		 // Set world properties.
		 hkpWorldCinfo winfo;

		 winfo.m_simulationType = hkpWorldCinfo::SIMULATION_TYPE_MULTITHREADED;
		 winfo.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM);
		 winfo.m_gravity.set(0, -9.8f, 0); // Default value, you could not write this
		 winfo.m_collisionTolerance = 0.1f; // Default value 

		 // Flag objects that fall "out of the world" to be automatically removed - just necessary for this physics scene
		 winfo.m_broadPhaseBorderBehaviour = hkpWorldCinfo::BROADPHASE_BORDER_REMOVE_ENTITY;
		 winfo.setBroadPhaseWorldSize(1000.0f); // Default value
		 // We need an hkpTreeBroadPhase to perform culling.
		 winfo.m_broadPhaseType = hkpWorldCinfo::BROADPHASE_TYPE_TREE;

		 world = new hkpWorld( winfo );

		 // When the simulation type is SIMULATION_TYPE_MULTITHREADED, in the debug build, the sdk performs checks
		 // to make sure only one thread is modifying the world at once to prevent multithreaded bugs. Each thread
		 // must call markForRead / markForWrite before it modifies the world to enable these checks.
		 world->markForWrite();

		 // Register all collision agents
		 // It's important to register collision agents before adding any entities to the world.
		 hkpAgentRegisterUtil::registerAllAgents( world->getCollisionDispatcher() );

		 world->registerWithJobQueue( jobQueue );
		 //............................................................

		 // Add the static environment to the world.
		 ptr_environment->Load_Create_Environment( MAP_WTEST_1_HAVOK, world );

		 // Create the scenery listener.
		 Physics_Scenery::CreateListener_EndOfStep();

		 // Add any game rigid bodies from the world manager.
		 add_RigidBody_Scenery();
		 add_RigidBody_Player();

		 // Register all the physics viewers
		 hkpPhysicsContext::registerAllPhysicsProcesses();

		 // Context.....................................................

		 //
		 // Initialize the VDB
		 //
		 hkArray<hkProcessContext*> contexts;

		 // <PHYSICS-ONLY>: Register physics specific visual debugger processes
		 // By default the VDB will show debug points and lines, however some products such as physics and cloth have additional viewers
		 // that can show geometries etc and can be enabled and disabled by the VDB app.
		 hkpPhysicsContext* context;
		 {
			 // The visual debugger so we can connect remotely to the simulation
			 // The context must exist beyond the use of the VDB instance, and you can make
			 // whatever contexts you like for your own viewer types.
			 context = new hkpPhysicsContext();
			 hkpPhysicsContext::registerAllPhysicsProcesses(); // all the physics viewers
			 context->addWorld(world); // add the physics world so the viewers can see it
			 contexts.pushBack(context);

			 // Now we have finished modifying the world, release our write marker.
			 world->unmarkForWrite();
		 }

		 // Set up the visual debugger
		 hkVisualDebugger* visualDebugger = new hkVisualDebugger(contexts);
		 visualDebugger->serve();

		 //.............................................................


		 // A stopwatch (64-bit timer) for waiting until the real time has passed
		 hkStopwatch stopWatch;
		 stopWatch.start();
		 hkReal lastTime = stopWatch.getElapsedSeconds();

		 // Update as if running at 60 frames per second.
		 const int numStepsPerSecond = 60;
		 const hkReal timeStep = 1.0f / hkReal(numStepsPerSecond);



		 // Loop.
		 while( running ){

			 Update( timeStep );

			 // Do a simulation step
			 world->stepDeltaTime(timeStep);
			 // Step the debugger
			 visualDebugger->step();

			 // Pause until the actual time has passed
			 while (stopWatch.getElapsedSeconds() < lastTime + timeStep);
			 lastTime += timeStep;
		 }

		 // Destroy. 

		 delete ptr_player_character;

		 // <PHYSICS-ONLY>: cleanup physics
		 {
			 world->markForWrite();
			 world->removeReference();
		 }
		 visualDebugger->removeReference();


		 // Contexts are not reference counted at the base class level by the VDB as
		 // they are just interfaces really. So only delete the context after you have
		 // finished using the VDB.
		 context->removeReference();

		 delete jobQueue;

		 //
		 // Clean up the thread pool
		 //
		 threadPool->removeReference();

		 hkBaseSystem::quit();
		 hkMemoryInitUtil::quit();
	 }

}
